//
//  main.cpp
//  SMCABC
//
//  Created by Paul Marjoram on 10/23/12.
//  Copyright (c) 2012 Paul Marjoram. All rights reserved.
//

using namespace std;

//#include "tchar.h"
#include<iostream>
#include <iomanip>
//#include<sstream>
#include<string>
#include<fstream>
#include<math.h>
#include <cassert>
#include <time.h>

#include <stdio.h>
#include <stdlib.h>
#include<gsl/gsl_randist.h>
#include "flysim.h"

using namespace std;

//ofstream ofile("SMCout1.txt",ios::out);
//const int ciNoOfParameters=3;
//float dParameterLowerBounds[ciNoOfParameters]={0,0,0};
//float dParameterUpperBounds[ciNoOfParameters]={10,10,10};
//const int ciDimensionOfData=3;
//const int ciNoOfAcceptancesPerGeneration=1000;
//int iNumberOfGenerationsForSMCAnalysis=20;
//float ThisGensParams[ciNoOfAcceptancesPerGeneration][ciNoOfParameters];
//int TempArray[ciNoOfParameters];
//bool bDoPureRejection=0; // generally this will be set to 0, but if we set it to one it will do rejection without SMC ( y setting the number of generations equal to 1
//
//
//
//// Some useful function declarations follow
//
//long int ranseed= -153425;
//
////*********************************************
///*  Random number generator from Numerical recipes (Ed 2, ran1) */
//#define IA 16807
//#define IM 2147483647
//#define AM (1.0/IM)
//#define IQ 127773
//#define IR 2836
//#define NTAB 32
//#define NDIV (1+(IM-1)/NTAB)
//#define EPS 1.2e-7
//#define RNMX (1.0-EPS)
//float rand01(long *idum)
//{
//	int j;
//	long k;
//	static long iy=0;
//	static long iv[NTAB];
//	float temp;
//	if(*idum<=0 || !iy) {
//		if(-(*idum) < 1) *idum=1;
//        else *idum= -(*idum);
//		for(j=NTAB+7;j>=0;j--) {
//			k=(*idum)/IQ;
//			*idum=IA*(*idum-k*IQ) -IR*k;
//			if(*idum < 0) *idum += IM;
//			if(j <NTAB) iv[j]= *idum;
//		}
//		iy=iv[0];
//	}
//	k=(*idum)/IQ;
//	*idum=IA*(*idum-k*IQ)-IR*k;
//	if(*idum < 0) *idum += IM;
//	j=iy/NDIV;
//	iy=iv[j];
//	iv[j]= *idum;
//	if((temp=AM*iy) > RNMX) return RNMX;
//	else return temp;
//}
//#ifdef TRAND
//main() {
//	int i,bin[10];
//	long dummy= -9284322;
//	for(i=0;i<10;i++) bin[i]=0;
//	for(i=0;i<20000;i++)
//		bin[(int)10*rand01(&dummy)]++;
//	for(i=0;i<10;i++)
//		printf("\n\t%d %lf",i,bin[i]/20000.0);
//}
//#endif
////**********************************************
//
//
//#define SQRT_2 sqrt(2.0)
//#define SQRT_PI sqrt(3.14159265)
//
///*Generator of normally distributed random variable from Numerical recipes (Ed 2, gasdev) */
//float gasdev(long *idum)
//{
//	static int iset=0;
//	static float gset;
//	float fac,rsq,v1,v2;
//	if (iset==0) {
//		do{
//			v1=2.0*rand01(&ranseed)-1.0;
//			v2=2.0*rand01(&ranseed)-1.0;
//			rsq=v1*v1+v2*v2;
//		}while(rsq>=1.0 ||rsq==0.0);
//		fac=sqrt(-2.0*log(rsq)/rsq);
//		gset=v1*fac;
//		iset=1;
//		return v2*fac;
//	}else{
//		iset=0;
//		return gset;
//	}
//}
//
//float NormalRV(float Mean, float StdDev)
//{
//    return Mean+StdDev*gasdev(&ranseed);
//}
//
////*******************************************************
//float erfcc(float x)
//{
//	float t,z,ans;
//	
//	z=fabs(x);
//	t=1.0/(1.0+0.5*z);
//	ans=t*exp(-z*z-1.26551223+t*(1.00002368+t*(0.37409196+t*(0.09678418+
//                                                             t*(-0.18628806+t*(0.27886807+t*(-1.13520398+t*(1.48851587+
//                                                                                                            t*(-0.82215223+t*0.17087277)))))))));
//	return x >= 0.0 ? ans : 2.0-ans;
//}
//
////********************************************************
//
//float P_Normal_greater_than_x(float xx, float mean, float sigma)
//{
//	// returns normalising constants for normal random variables
//	xx=xx-mean;
//	xx=xx/sigma;
//	return erfcc(xx/SQRT_2)/2.0;
//}
//
////********************************************************
//
//float NormalDensity(float x, float mean, float StdDev)
//{
//	float dDen;
//	dDen=1.0/(StdDev*SQRT_2*SQRT_PI);
//	dDen=dDen*exp(-1*(x-mean)*(x-mean)/(2*StdDev*StdDev));
//	return dDen;
//}
//
////*******************************************************
//
//float ExponentialRV(float dLambda) // generates exponential random variables
//{
//	float p,x;
//	p=rand01(&ranseed);
//	x=-1*log(p)/dLambda;
//	return x;
//}
//
////*******************************************************
////*******************************************************
////********classe implemetations follow*******************
////*******************************************************
////*******************************************************
//
//
SMC::~SMC(){
}

SMC::SMC(){
  iNumberOfGenerationsForSMCAnalysis=20;
  dParameterLowerBounds[0]=0;
  dParameterLowerBounds[1]=0;
  dParameterLowerBounds[2]=0;
  dParameterUpperBounds[0]=10;
  dParameterUpperBounds[1]=10;
  dParameterUpperBounds[2]=10;
  TestParamValues[0]=1;
  TestParamValues[1]=3;
  TestParamValues[2]=5;
  rg = gsl_rng_alloc(gsl_rng_mt19937);
  gsl_rng_set(rg,ranseed);
  cerr<<"RNG created of type "<<gsl_rng_name (rg)<<
  " using seed "<<ranseed<<endl;

}

void SMC::PrintParameterValues(bool bPrintToFile)
{
    if (bPrintToFile==0)
    {
        std::cout<<"\n*************Parameter values follow***********";
        for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
            std::cout<<endl;
            for ( int j=0; j< ciNoOfParameters; j++) {
                std::cout<<NewAcceptedParameterValues[i][j]<<" ";
            }
            std::cout<<"     weight: "<<dWeightsOfOldParametersValues[i]<<" "<<dWeightsOfNewParametersValues[i];
        }
        std::cout<<"\n***********************************************";
    }
    else
    {
        for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
            ofile<<endl;
            for ( int j=0; j< ciNoOfParameters; j++) {
                ofile<<NewAcceptedParameterValues[i][j]<<" ";
            }
            ofile<<"      "<<dWeightsOfOldParametersValues[i]<<" "<<dWeightsOfNewParametersValues[i];
        }
    }
}


void SMC::BuildTestData()
{
    for (int i=0; i<ciNoOfTestDatasets; i++)
    {
        TestData[i][0]=TestParamValues[0]+gsl_ran_gaussian(rg,StdDev);;//NormalRV(TestParamValues[0], StdDev);
        TestData[i][1]=TestParamValues[1]+gsl_ran_gaussian(rg,StdDev);; //sNormalRV(TestParamValues[1], StdDev);
        TestData[i][2]=TestParamValues[2]+gsl_ran_gaussian(rg,StdDev);//NormalRV(TestParamValues[2], StdDev);
    }
}

void SMC::PrintTestData()
{
    std::cout<<"\n*************Test data follows***********";
    for ( int i = 0; i < ciNoOfTestDatasets; ++i ) {
        std::cout<<endl;
        for ( int j=0; j< ciDimensionOfData; j++) {
            std::cout<<TestData[i][j]<<"  ";
        }
    }
    std::cout<<"\n*****************************************";
    
}

void SMC::RunSMCAnalysis()
{
    
    BuildTestData();
    PrintTestData();
    if (bDoPureRejection==1)
        iNumberOfGenerationsForSMCAnalysis=1;
    for (int i=0;i<iNumberOfGenerationsForSMCAnalysis;i++)
    {
        DoThisGenerationsRejection(i);
        UpdateParameterValuesAndWeights();
        //PrintParameterValues();
    }
    PrintParameterValues(0);
    PrintParameterValues(1);
}



void SMC::GenerateParameters(int iGen)
{
    float p,dProbSoFar;
    int i,j;
    if (iGen==0)
    {
        SampledParameters[0]=SampleFromParameterPrior(0);
        SampledParameters[1]=SampleFromParameterPrior(1);
        SampledParameters[2]=SampleFromParameterPrior(2);
    }
    else
    {
        for (i=0;i<ciNoOfParameters;i++)
        {
            p=gsl_rng_uniform(rg);
            //p=rand01(&ranseed);
            dProbSoFar=0;
            j=-1;
            while (dProbSoFar<p)
            {
                j++;
                dProbSoFar+=dWeightsOfOldParametersValues[j];
            }
            assert(j<ciNoOfAcceptsNeeded);
            SampledParameters[i]=OldAcceptedParameterValues[j][i];
            // add noise
            //SampledParameters[i]+=NormalRV(0, StdDev);
		SampledParameters[i]+=gsl_ran_gaussian(rg,StdDev);
        }
    }
}

void SMC::GenerateData()
{
	GeneratedData[0]=SampledParameters[0]+gsl_ran_gaussian(rg,StdDev);
	GeneratedData[1]=SampledParameters[1]+gsl_ran_gaussian(rg,StdDev);
	GeneratedData[2]=SampledParameters[2]+gsl_ran_gaussian(rg,StdDev);

    //GeneratedData[0]=NormalRV(SampledParameters[0],StdDev);
    //GeneratedData[1]=NormalRV(SampledParameters[1],StdDev);
    //GeneratedData[2]=NormalRV(SampledParameters[2],StdDev);
}

float SMC::MeasureDistanceBetweenDataAndParameters()
{
    float d=0;
    int i,j;
    for (j=0;j<ciNoOfTestDatasets;j++)
        for (i=0; i<ciDimensionOfData;i++)
            d+=(GeneratedData[i]-TestData[j][i])*(GeneratedData[i]-TestData[j][i]);
    return d;
}

void SMC::DoThisGenerationsRejection(int iGen)
{
    int iAccepts=0,iAttempts=0,j;
    float dDist;
    while ((iAccepts<ciNoOfAcceptsNeeded)&&(iAttempts<iMaxNumberOfAttempts))
    {
        GenerateParameters(iGen);
        GenerateData();
        dDist=MeasureDistanceBetweenDataAndParameters();
        if (dDist<dRejectionThreshold)
        {
            // accept
            for (j=0;j<ciNoOfParameters;j++)
                NewAcceptedParameterValues[iAccepts][j]=SampledParameters[j];
                           iAccepts++;
        }
        iAttempts++;
    }
    if (iAttempts==iMaxNumberOfAttempts)
    {
        std::cout<<"\nToo many attempts needed. Exit.";
        PrintParameterValues(0);
        exit(0);
    }
    CalculateParameterWeights(iGen);
    std::cout<<"\nGeneration "<<iGen<<" required "<<iAttempts<<" iterations";
}

void SMC::UpdateParameterValuesAndWeights()
{
    int i,j;
    for (i=0;i<ciNoOfAcceptsNeeded;i++)
    {
        dWeightsOfOldParametersValues[i]=dWeightsOfNewParametersValues[i];
        for (j=0;j<ciNoOfParameters;j++)
            OldAcceptedParameterValues[i][j]=NewAcceptedParameterValues[i][j];
    }
}

void SMC::CalculateParameterWeights(int iGen)
{
    int i,j,k;
    float dDensity,dDen,dTotalDensity,dSumOfWeights=0;
    // for each set of New Parameter values
    for (k=0;k<ciNoOfAcceptsNeeded;k++)
    {
        if (iGen==0)
        {
            dWeightsOfNewParametersValues[k]=1.0/ciNoOfAcceptsNeeded;
            dSumOfWeights=1;
        }
        else
        {
            dTotalDensity=1;
            // for each parameter
            for (i=0;i<ciNoOfParameters;i++)
            {
                dDensity=0;
                // calculate the density of generating this value - need to go through each old parameter value to to this
                for (j=0;j<ciNoOfAcceptsNeeded;j++)
                {
                    dDen=gsl_ran_gaussian_pdf(NewAcceptedParameterValues[k][i]-OldAcceptedParameterValues[j][i],StdDev);
                    //dDen=NormalDensity(NewAcceptedParameterValues[k][i], OldAcceptedParameterValues[j][i], StdDev);
                    dDensity+=(dDen/ciNoOfAcceptsNeeded);
                }
                dTotalDensity=dTotalDensity*dDensity;
            }
            dWeightsOfNewParametersValues[k]=dTotalDensity;
        }
    }
    
    // They now need to be inverted to recognize the importance sampling correction
    if (iGen>0)
        for (k=0;k<ciNoOfAcceptsNeeded;k++)
        {
            dWeightsOfNewParametersValues[k]=1.0/dWeightsOfNewParametersValues[k];
            dSumOfWeights+=dWeightsOfNewParametersValues[k];
        }
    
    
    // Now normalize them go become probs ready for sampling in the next generation
    for (k=0;k<ciNoOfAcceptsNeeded;k++)
        dWeightsOfNewParametersValues[k]=dWeightsOfNewParametersValues[k]/dSumOfWeights;    
}

void SMC::InitializeArrays()
{
    for ( int i = 0; i < ciNoOfAcceptancesPerGeneration; ++i ) {
        dWeightsOfNewParametersValues[i]=1.0/((float)ciNoOfAcceptsNeeded);
        dWeightsOfOldParametersValues[i]=1.0/((float)ciNoOfAcceptsNeeded);
        for ( int j=0; j< ciNoOfParameters; j++) {
            OldAcceptedParameterValues[i][j] = -9;
            NewAcceptedParameterValues[i][j]=-9;
        }
    }
}

float SMC::SampleFromParameterPrior(int iWhichParameter)
{
    // we assume a uniform prior for now
    float dVal;
    dVal=gsl_rng_uniform(rg)*(dParameterUpperBounds[iWhichParameter]-dParameterLowerBounds[iWhichParameter]);
    //dVal=rand01(&ranseed)*(dParameterUpperBounds[iWhichParameter]-dParameterLowerBounds[iWhichParameter]);
    return dVal;
}


int main2(int argc, const char * argv[])
{

    // insert code here...
    SMC MyTester;
    MyTester.InitializeArrays();
    MyTester.PrintParameterValues(0);
    MyTester.RunSMCAnalysis();
   // MyTester.PrintParameterValues(0);
    MyTester.PrintParameterValues(1);
   std::cout << "\nHello, World!\n";
    return 0;
}

